#ifndef ROTATION_MATRIX_H
#define ROTATION_MATRIX_H

#include "../WindowsCompatibility/Physx.h"
#include "StringBuilder.h"

static const NxVec3 zeroVec3(0, 0, 0);

static void rotationVectorToMatrix(const NxVec3& v, NxMat33* matrix) {
	const NxReal cosX = cos(v.x);
	const NxReal cosY = cos(v.y);
	const NxReal cosZ = cos(v.z);

	const NxReal sinX = sin(v.x);
	const NxReal sinY = sin(v.y);
	const NxReal sinZ = sin(v.z);

	const NxReal sinXY = sinX * sinY;
	const NxReal cosXsinZ = cosX * sinZ;
	const NxReal cosXZ = cosX * cosZ;

	matrix->setRow(0, NxVec3(cosY*cosZ, -cosXsinZ + sinXY*cosZ,  sinX*sinZ + cosXZ*sinY));
	matrix->setRow(1, NxVec3(cosY*sinZ,  cosXZ + sinXY*sinZ, -sinX*cosZ + cosXsinZ*sinY));
	matrix->setRow(2, NxVec3(-sinY, sinX*cosY, cosX*cosY));
}

static bool operator==(const NxMat34& a, const NxMat34& b) {
	return a.M == b.M && a.t == b.t;
}

static StringBuilder& operator<<(StringBuilder& sb, const NxVec3& v) {
	sb << "(" << v.x << ", " << v.y << ", " << v.z << ")";
	return sb;
}

#endif //ROTATION_MATRIX_H
